#include <stm32f4xx.h>
#include <stdio.h>
#include <stdlib.h>
#include "led.h"
#include "cam.h"
#include "com.h"
#include "motor.h"
#include "img processing.h"
#include "img processing2.h"

void comCallBack(uint16_t type, void* data, uint16_t size);

int main (void)
{
	uint32_t i=0;
	
	// Initialize components
	ledInit();
	motorInit();
	camInit();
	comInit(comCallBack);
	
	imgPInit();
	imgP2Init();
	
	for(i=16800000;i>0;i--);
	for(i=16800000;i>0;i--);
	
	/*
	motorPointTurn(TURN90R);
	motorWaitWhileDriving();
	for(i=16800000;i>0;i--);
	
	motorPointTurn(TURN180L);
	motorWaitWhileDriving();
	for(i=16800000;i>0;i--);
	
	motorPointTurn(TURN90R);
	motorWaitWhileDriving();
	for(i=16800000;i>0;i--);
	
	motorDriveDistance(BACKWARD, DIST10CM);
	motorWaitWhileDriving();
	for(i=16800000;i>0;i--);
	
	motorDriveDistance(FORWARD, DIST10CM);
	motorWaitWhileDriving();
	for(i=16800000;i>0;i--);
	*/
	
	while(1)
	{
		motorWaitWhileDriving();
		
		SET_LED(LED_RUN);
		for(i=168000;i>0;i--);
		RESET_LED(LED_RUN);
		for(i=168000;i>0;i--);
		
		// Img process 1: Wall + Color detection 
		image_t* img = camGetFrame();
	
		if(img != NULL)
			imgP1(img);
			
		// Img process 2: Object Recognition
		if(imgP1ColorDetected())
		//if(1)
		{
			img = camGetFrame();
			if(img != NULL)
				imgP2(img);
		}
	}
}

void comCallBack(uint16_t type, void* data, uint16_t size)
{
	if(type == TYPE_START)
	{
		//start exploring
	}
	else if(type == TYPE_STOP)
	{
		//stop exploring
	}
	else if(type == TYPE_MOTOR_SET)
	{
		int8_t *pData = (int8_t*)data;
		uint32_t speed = ((((uint8_t)pData[2])|((uint8_t)pData[3]<<8))&0xFFFF);
		motorSetSpeed((int8_t)pData[0],(int8_t)pData[1],speed);
		//Don't call motorWaitWhileDriving() here!
	}
	else if(type == TYPE_FRONTLED_SET)
	{
		uint8_t *pData = (uint8_t*)data;
		if(pData[0] == 0x00)
			SET_LED(LED_FRONT);
		else
			RESET_LED(LED_FRONT);
	}
	else if(type == TYPE_RESET_COM) //reset communication
	{
		uint8_t *pData = (uint8_t*)data;
		uint32_t i, j;
		
		if(size == 2){
			if(pData[0] == 0x55){
				if(pData[1] == 0xAA){
					for(j=3;j>0;j--){ //play a nice led patern while the app resets communication
						for(i=0;i<16800000; i++);
						
						SET_LED(LED_OBJ1);
						for(i=0;i<1700000; i++);
						SET_LED(LED_OBJ2);
						for(i=0;i<1700000; i++);
						SET_LED(LED_OBJ3);
						for(i=0;i<1700000; i++);
						
						RESET_LED(LED_OBJ1);
						for(i=0;i<1700000; i++);
						RESET_LED(LED_OBJ2);
						for(i=0;i<1700000; i++);
						RESET_LED(LED_OBJ3);
						for(i=0;i<1700000; i++);
					}
				}
			}
		}
	}
	else{
	}
	//etc...
}
